﻿using System;
using System.Timers;
using System.IO.Ports;
using System.Collections.Generic;
using MECopter.Logging;
using MECopter.Quadrocopter;
using MECopter.Configuration.GroundControl;
using System.Collections.ObjectModel;
namespace MECopter.Communication
{
    /// <summary>
    /// Wrapps the SerialPort class and does the communication with the Arduino board
    /// </summary>
    class SerialWrapper
    {
        #region Fields
        private SerialPort serial;

        /// <summary>
        /// Buffer which contains incomming data
        /// </summary>
        public ObservableCollection<ReadData> ReadBuffer;
        
        /// <summary>
        /// Buffer which contains data that is supposed to be sent
        /// </summary>
        public ObservableCollection<SendData> WriteQueue;

        public bool Opened;
        #endregion

        #region Constructor
        /// <summary>
        /// Creats a object of MECopter.Communication.SerialWrapper
        /// </summary>
        public SerialWrapper()
        {
            ReadBuffer = new ObservableCollection<ReadData>();
            WriteQueue = new ObservableCollection<SendData>();
            WriteQueue.CollectionChanged += WriteQueue_CollectionChanged;
            Opened = false;
        }       
        #endregion        

        #region Methods
        /// <summary>
        /// Opens the connection
        /// </summary>
        /// <param name="portName">COM-Port to use</param>
        /// <param name="baudRate">Baudrate to use</param>
        public void Open(string portName, int baudRate)
        {
            try
            {
                serial = new SerialPort(portName, baudRate);
                serial.DataReceived += new SerialDataReceivedEventHandler(serial_DataReceived);
                serial.Open();
                
                Opened = true;
            }
            catch(Exception ex)
            {
                Log.Error(ex.ToString());
                Opened = false;
            }
        }

        /// <summary>
        /// Closes the connection
        /// </summary>
        public void Close()
        {
            
            serial.Close();
            Opened = false;
        }
        #endregion

        #region Eventhandler

        private void serial_DataReceived(object sender, SerialDataReceivedEventArgs e)
        {

            try
            {
                while (serial.BytesToRead > 0) //Read until there are no more bytes
                {
                    string incommingString = serial.ReadLine();
                    //Log.Debug(incommingString); //Only for debuing purposes
                    char[] cmd = incommingString.ToCharArray();
                    ReadData data;
                    switch (cmd[0])
                    {
                        case (char)ReadDataType.YPR: //YPR     +:Y:P:R
                            Angle ypr = new Angle();
                            ypr.Yaw = float.Parse(incommingString.Split(':')[1], System.Globalization.CultureInfo.CreateSpecificCulture("en-us")); //Yaw
                            ypr.Pitch = float.Parse(incommingString.Split(':')[2], System.Globalization.CultureInfo.CreateSpecificCulture("en-us")); //Pitch
                            ypr.Roll = float.Parse(incommingString.Split(':')[3], System.Globalization.CultureInfo.CreateSpecificCulture("en-us")); //Roll
                            data = new ReadData(ReadDataType.YPR, ypr);
                            ReadBuffer.Add(data);
                            break;

                        case (char)ReadDataType.ESC: //ESC     #:ESC0:ESC1:ESC2:ESC3
                            float[] esc = new float[4];

                            esc[0] = (float)Convert.ToDouble(incommingString.Split(':')[1], System.Globalization.CultureInfo.CreateSpecificCulture("en-us"));
                            esc[1] = (float)Convert.ToDouble(incommingString.Split(':')[2], System.Globalization.CultureInfo.CreateSpecificCulture("en-us"));
                            esc[2] = (float)Convert.ToDouble(incommingString.Split(':')[3], System.Globalization.CultureInfo.CreateSpecificCulture("en-us"));
                            esc[3] = (float)Convert.ToDouble(incommingString.Split(':')[4], System.Globalization.CultureInfo.CreateSpecificCulture("en-us"));
                            data = new ReadData(ReadDataType.ESC, esc);
                            ReadBuffer.Add(data);
                            break;

                        case (char)ReadDataType.LoopTime: //LoopTime    ?:TIME
                            ulong looptime = ulong.Parse(incommingString.Split(':')[1]);
                            data = new ReadData(ReadDataType.LoopTime, looptime);
                            ReadBuffer.Add(data);
                            break;

                        case (char)ReadDataType.Accu: //Accu    &:CELL1:CELL2
                            //TODO
                            break;

                        case (char)ReadDataType.PIDValues: //Accu    &:CELL1:CELL2
                            float[] values = new float[3];
                            values[0] = (float)Convert.ToDouble(incommingString.Split(':')[1], System.Globalization.CultureInfo.CreateSpecificCulture("en-us"));
                            values[1] = (float)Convert.ToDouble(incommingString.Split(':')[2], System.Globalization.CultureInfo.CreateSpecificCulture("en-us"));
                            values[2] = (float)Convert.ToDouble(incommingString.Split(':')[3], System.Globalization.CultureInfo.CreateSpecificCulture("en-us"));
                            data = new ReadData(ReadDataType.PIDValues, values);
                            ReadBuffer.Add(data);
                            break;

                        case (char)ReadDataType.PIDParameter:
                            Log.Debug(incommingString);
                            PIDParameters parameters = new PIDParameters();
                            parameters.YawPGain = (float)Convert.ToDouble(incommingString.Split(':')[1], System.Globalization.CultureInfo.CreateSpecificCulture("en-us"));
                            parameters.YawIGain = (float)Convert.ToDouble(incommingString.Split(':')[2], System.Globalization.CultureInfo.CreateSpecificCulture("en-us"));
                            parameters.YawDGain = (float)Convert.ToDouble(incommingString.Split(':')[3], System.Globalization.CultureInfo.CreateSpecificCulture("en-us"));

                            parameters.PitchPGain = (float)Convert.ToDouble(incommingString.Split(':')[4], System.Globalization.CultureInfo.CreateSpecificCulture("en-us"));
                            parameters.PitchIGain = (float)Convert.ToDouble(incommingString.Split(':')[5], System.Globalization.CultureInfo.CreateSpecificCulture("en-us"));
                            parameters.PitchDGain = (float)Convert.ToDouble(incommingString.Split(':')[6], System.Globalization.CultureInfo.CreateSpecificCulture("en-us"));

                            parameters.RollPGain = (float)Convert.ToDouble(incommingString.Split(':')[7], System.Globalization.CultureInfo.CreateSpecificCulture("en-us"));
                            parameters.RollIGain = (float)Convert.ToDouble(incommingString.Split(':')[8], System.Globalization.CultureInfo.CreateSpecificCulture("en-us"));
                            parameters.RollDGain = (float)Convert.ToDouble(incommingString.Split(':')[9], System.Globalization.CultureInfo.CreateSpecificCulture("en-us"));

                            MECopter.Configuration.Quadrocopter.QuadrocopterConfig.pidParameters = parameters;
                            break;
                    }
                }
            }

            catch (Exception ex)
            {
                Log.Error(ex.ToString());
            }
        }

        void WriteQueue_CollectionChanged(object sender, System.Collections.Specialized.NotifyCollectionChangedEventArgs e)
        {
            for (int i = 0; i < WriteQueue.Count; i++)
            {
                try
                {
                    //Log.Error("SEND");
                    switch (WriteQueue[i].Type)
                    {

                        case SendDataType.ESCEnable: //!:state
                            bool state = (bool)WriteQueue[i].Data;
                            serial.Write("!:");
                            if (state)
                                serial.Write("1;");
                            else
                                serial.Write("0;");
                            break;

                        case SendDataType.RC: //#RC:speed:yaw:pitch:roll                   

                            Quadrocopter.ControllValues ctrl = (Quadrocopter.ControllValues)WriteQueue[i].Data;
                            serial.Write(((char)SendDataType.RC).ToString()); serial.Write(":");
                            if (ctrl.Speed >= 0)
                            {
                                serial.Write(ctrl.Speed.ToString(System.Globalization.CultureInfo.CreateSpecificCulture("en-us")) + ':');
                            }
                            else
                            {
                                serial.Write("0" + ':');
                            }
                            serial.Write(ctrl.Yaw.ToString(System.Globalization.CultureInfo.CreateSpecificCulture("en-us")) + ':');
                            serial.Write(ctrl.Pitch.ToString(System.Globalization.CultureInfo.CreateSpecificCulture("en-us")) + ':');
                            serial.Write(ctrl.Roll.ToString(System.Globalization.CultureInfo.CreateSpecificCulture("en-us")));
                            serial.Write(";");
                            break;

                        case SendDataType.PIDParameter:
                            PIDParameters parameters = (PIDParameters)WriteQueue[i].Data;
                            serial.Write(((char)SendDataType.PIDParameter).ToString()); serial.Write(":");

                            serial.Write(parameters.YawPGain.ToString(System.Globalization.CultureInfo.CreateSpecificCulture("en-us")) + ':');
                            serial.Write(parameters.YawIGain.ToString(System.Globalization.CultureInfo.CreateSpecificCulture("en-us")) + ':');
                            serial.Write(parameters.YawDGain.ToString(System.Globalization.CultureInfo.CreateSpecificCulture("en-us")) + ':');

                            serial.Write(parameters.PitchPGain.ToString(System.Globalization.CultureInfo.CreateSpecificCulture("en-us")) + ':');
                            serial.Write(parameters.PitchIGain.ToString(System.Globalization.CultureInfo.CreateSpecificCulture("en-us")) + ':');
                            serial.Write(parameters.PitchDGain.ToString(System.Globalization.CultureInfo.CreateSpecificCulture("en-us")) + ':');

                            serial.Write(parameters.RollPGain.ToString(System.Globalization.CultureInfo.CreateSpecificCulture("en-us")) + ':');
                            serial.Write(parameters.RollIGain.ToString(System.Globalization.CultureInfo.CreateSpecificCulture("en-us")) + ':');
                            serial.Write(parameters.RollDGain.ToString(System.Globalization.CultureInfo.CreateSpecificCulture("en-us")) + ':');
                            serial.Write(";");
                            break;
                        case SendDataType.Request:
                            serial.Write(((char)SendDataType.Request).ToString()); serial.Write(":" + ((char)((SendDataType)WriteQueue[i].Data)).ToString());
                            break;

                    }

                    WriteQueue.Remove(WriteQueue[i]);
                }
                catch (Exception ex)
                {
                    Log.Error(ex);
                }
            }
        }

        #endregion

    }
}
